% by jasper nevens
clc
clear all;
close all;
tic()

% sensor 1
d1=56.6;
a1=7*pi/4;

% sensor 2
d2=56.6;
a2=5*pi/4;

%% Methode 1 - relative position
dn1 = -5;
de1 = 0;
dn2 = -5;
de2 = 0;

A = [ 1 0 -d1*sin(a1) ; 0 1 d1*cos(a1) ; 1 0 -d2*sin(a2) ; 0 1 d2*cos(a2)];
AI=pinv(A)
a = [ de1*cos(a1) - dn1*sin(a1) ; de1*sin(a1) + dn1*cos(a1) ; de2*cos(a2) - dn2*sin(a2) ; de2*sin(a2) + dn2*cos(a2)]

u=AI*a

dx=u(1);
dy=u(2);
dt=u(3);

%% death reconing - absolute position
startx=100;
starty=100;
startt=pi/2;

R=[startx + dx*cos(startt) - dy*sin(startt) ; starty + dx*sin(startt) + dy*cos(startt) ; startt + dt]
X=R(1);
Y=R(2);
Ot=R(3);

toc()
